package com.gitee.wsl.mathematics.complex.quaternion.ext

import com.gitee.wsl.mathematics.complex.ComplexField
import com.gitee.wsl.mathematics.complex.toComplex
import com.gitee.wsl.mathematics.vector.ext.dot
import com.gitee.wsl.mathematics.complex.quaternion.Quat
import kotlin.math.acos
import kotlin.math.cos
import kotlin.math.sin


fun Quat.ln(): Quat {
    val nu2 = x * x + y * y + z * z

    if (nu2 == 0.0)
        return if (w > 0)
            Quat(kotlin.math.ln(w), 0, 0, 0)
        else {
            val l = ComplexField.ln(w.toComplex())
            Quat(l.re, l.im, 0, 0)
        }

    val a = w
    check(nu2 > 0)
    val n = sqrt(a * a + nu2)
    val th = acos(a / n) / sqrt(nu2)
    return Quat(kotlin.math.ln(n), th * x, th * y, th * z)
}

fun Quat.exp(): Quat {
    val un = x * x + y * y + z * z
    if (un == 0.0) return Quat(kotlin.math.exp(w))
    val n1 = sqrt(un)
    val ea = kotlin.math.exp(w)
    val n2 = ea * sin(n1) / n1
    return Quat(ea * cos(n1), n2 * x, n2 * y, n2 * z)
}

inline fun Quat.Companion.func(callback: (Int) -> Double) = Quat(callback(0), callback(1), callback(2), callback(3))
inline fun Quat.Companion.func(l: Quat, r: Quat, func: (l: Double, r: Double) -> Double) = Quat(
    func(l.x, r.x),
    func(l.y, r.y),
    func(l.z, r.z),
    func(l.w, r.w)
)
fun Quat.Companion.slerp(left: Quat, right: Quat, t: Float): Quat {
    var tleft = left.normalized
    var tright = right.normalized

    var dot = tleft dot right

    if (dot < 0.0f) {
        tright = -tright
        dot = -dot
    }

    if (dot > 0.99995f) return func(tleft, tright) { l, r -> l + t * (r - l) }

    val angle0 = acos(dot)
    val angle1 = angle0 * t

    val s1 = sin(angle1) / sin(angle0)
    val s0 = cos(angle1) - dot * s1

    return func(tleft, tright) { l, r -> ((s0 * l) + (s1 * r)) }
}

fun Quat.Companion.nlerp(left: Quat, right: Quat, t: Double): Quat {
    val sign = if (left dot right < 0) -1 else +1
    return func { ((1f - t) * left.proxy[it] + t * right.proxy[it] * sign) }.normalized
}

fun Quat.Companion.interpolated(left: Quat, right: Quat, t: Float): Quat = slerp(left, right, t)
